from concurrent.futures import thread
from glob import glob
from pickle import FALSE
import queue
import socket
import threading
import time
import lib.tcp_server as tcp_server
from lib.log_udp_server import DEBUG,get_current_ms
import lib.MavlinkParse as mavlink

UDP_IMPORT_MAVLINK = False  #使能接收客户端数据，并解析mavlink

udp_client_map = {}
udp_client_map_lock = threading.Lock()
udp_send_queue = queue.Queue(0)
udp_rec_queue = queue.Queue(0)
auto_test_enable = False
udp_error = False
udp_mav = mavlink.MavlinkParse()

### 设置服务器自动解析mavlink数据包
def set_mavlink_enable(t):
    global UDP_IMPORT_MAVLINK
    UDP_IMPORT_MAVLINK = t

### 是否开启自动回测功能
def set_auto_test_enable(t):
    global auto_test_enable
    auto_test_enable = t

### 更新UDP发送数据队列
def udp_server_send_data(data):
    global udp_send_queue
    if len(udp_client_map)>0:
        udp_send_queue.put(data)

def udp_server_send_clear():
    global udp_send_queue
    udp_send_queue.queue.clear()

### 检查udp客户端列表，存在超时就删除
### map: 客户端列表字典，键值由客户端地址、端口组成，，值为(addr(地址，端口),最后连接服务器时间毫秒)
### lock: 字典操作线程安全锁
def check_client_map():
    while True:
        if len(udp_client_map)>0:
            udp_client_map_lock.acquire()
            cmap = udp_client_map.copy()
            udp_client_map_lock.release()

            now = get_current_ms()
            for k,v in cmap.items():
                if now>v[1] and now-v[1]>20000: #如果超过20S没有接收到该客户端数据，则选择删除该客户端
                    udp_client_map_lock.acquire()
                    del udp_client_map[k]
                    udp_client_map_lock.release()
                    
                    msg = "APP-->UDP Client Timeout: remove (%s->%d)!!!" % (k, len(udp_client_map))                
                    DEBUG(msg, True, True)        
        time.sleep(0.5)

### 更新udp客户端列表
def update_client_map(addr):
    name = "%s->%d" % (addr[0], addr[1])
    if name not in udp_client_map.keys():
        msg = "APP-->UDP Client Map: add (%s)" % name
        DEBUG(msg, True, True)
    udp_client_map_lock.acquire()
    udp_client_map[name] = (addr, get_current_ms())
    udp_client_map_lock.release()

### UDP服务器将TCP服务器接收到的数据转发到客户端,传入udp服务器对象
### 如果TCP服务器接收到了客户端数据，则将接收到的数据转发给所有的UDP客户端
def udp_send_task(udp):
    last_msg_time_ms = get_current_ms()
    send_count = 0
    while True:
        now = get_current_ms()
        if len(udp_client_map)>0:
            try:     
                while (not udp_send_queue.empty()):
                    data = udp_send_queue.get()[2]
                    if len(data)<=0:
                        continue

                    udp_client_map_lock.acquire()
                    cmap = udp_client_map.copy()
                    udp_client_map_lock.release()

                    send_msg_cmp = False
                    send_count = send_count + len(data)

                    # 将数据发送到每一个客户端
                    for k,v in cmap.items():
                        udp.sendto(data, v[0])
                        if now>last_msg_time_ms and now-last_msg_time_ms>=5000:
                            msg = ">>> UDP Server -> UDP Client (%s: %d / 5s)" % (k, send_count)
                            DEBUG(msg, True, True)
                            send_msg_cmp = True
                    if send_msg_cmp:
                        last_msg_time_ms = get_current_ms()
                        send_count = 0
            except:
                pass
                DEBUG("APP-->UDP Client error!!!", True, True)
        else:
            udp_send_queue.queue.clear()
  
        time.sleep(0.005)

### UDP接收数据，将来通过TCP服务器下发到设备
def udp_rec_task(udp):
    last_msg_time_ms = get_current_ms()
    rec_count = 0
    while True:
        now = get_current_ms()
        try:
            data, addr = udp.recvfrom(4096)
            if len(data)>0:
                udp_error = False
                update_client_map(addr)     #更新客户端列表
                
                # 分包最大230个字节
                rl = len(data)
                s = 0
                while 0!=rl:
                    sdata = bytes()
                    if rl>230:
                        sdata = data[s:s+230]
                        s = s + 230
                        rl = rl - 230
                    else:
                        sdata = data[s:s+rl]
                        rl = 0
                    udp_rec_queue.put((udp, addr, sdata))

                #udp_rec_queue.put((udp, addr, data))

                if auto_test_enable:
                    udp_send_queue.put((udp, addr, data))

                rec_count = rec_count + len(data)
                if now>last_msg_time_ms and now-last_msg_time_ms>=5000:
                    msg = ">>> UDP Client >> UDP Server(%s->%d: %d / 5s)" % (addr[0], addr[1], rec_count)
                    DEBUG(msg, True, True)
                    last_msg_time_ms = now
                    rec_count = 0

                if UDP_IMPORT_MAVLINK:
                    m = udp_mav.parse(data)
                    if m is not None:
                        msg = ">>> UDP Client >> UDP Server (mavlink): %s->(id=%d, len=%d)" % (m.name, m.get_header().msgId, m.get_header().mlen)
                        DEBUG(msg, True, True)
        except:
            if not udp_error:
                DEBUG("APP-->UDP Server Recvfrom Error!!!", True, True)
                udp_error = True

### UDP服务器开启
def udp_server_run(addr):
    global udp_server
    udp_server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    udp_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    udp_server.bind(addr)
    DEBUG("APP-->UDP server start listen:---", True, True)

    td1 = threading.Thread(target=udp_send_task, args=(udp_server,))
    td1.start()

    td2 = threading.Thread(target=udp_rec_task, args=(udp_server,))
    td2.start()

    td3 = threading.Thread(target=check_client_map)
    td3.start()

